#include "watch_dog/watch_dog.h"
#include "logger.h"
#include "utils/utils.h"
#include <thread>
#include <sys/wait.h>
#include <unistd.h>
#include "utils/common_utils.h"

WatchDog *WatchDog::getWatchDog()
{
    static WatchDog watchDog;
    return &watchDog;
}
WatchDog::WatchDog()
{
}
void WatchDog::createNode(const LaunchNode &launchNode)
{
    for (const auto &node : launchNode.nodeList)
    {
        ROS_INFO("node name: %s, pkg: %s, type: %s, args: %s",
                 node.name.c_str(), node.pkg.c_str(), node.type.c_str(), node.args.c_str());
        nodeInfoMap.emplace(generateKey(node), node); // 保存节点信息
    }

    // 创建进程
    for (const auto &node : launchNode.nodeList)
    {
        createNode(node);
    }
}
void WatchDog::createNode(const NodeInfo &nodeInfo)
{
    pid_t pid;
    std::string args = nodeInfo.args;
    args.append(std::string(" __name:=") + nodeInfo.name); // 节点名称修改

    bool printScreen = nodeInfo.printScreen;
    if (Utils::isRelease())
    {
        printScreen = false; // release模式，无论什么配置，都不允许打印屏幕日志
    }

    auto res = Utils::createProcess(&pid, nodeInfo.execPath, args, nodeInfo.envs, printScreen);
    if (0 != res.res)
    {
        ROS_ERROR("pkg: %s, type: %s, name: %s, file: %s, run fail, res: %d, err: %s", nodeInfo.pkg.c_str(),
                  nodeInfo.type.c_str(), nodeInfo.name.c_str(), nodeInfo.execPath.c_str(), res.res, res.errDesc.c_str());
    }
    else
    {
        ROS_INFO("run, pid: %ld, pkg: %s, type: %s, name: %s, file: %s", pid,
                 nodeInfo.pkg.c_str(), nodeInfo.type.c_str(), nodeInfo.name.c_str(), nodeInfo.execPath.c_str());
        savePid(nodeInfo, pid); // 保存pid，pid可能会变
    }
}
void WatchDog::recreateNode(const NodeInfo &nodeInfo)
{
    if (nodeInfo.respawn)
    {
        createNode(nodeInfo);
    }
}
void WatchDog::savePid(const NodeInfo &nodeInfo, pid_t pid)
{
    removePid(nodeInfo);
    pidMap.emplace(generateKey(nodeInfo), pid);
}
void WatchDog::removePid(const NodeInfo &nodeInfo)
{
    std::string key = generateKey(nodeInfo);
    if (pidMap.find(key) != pidMap.end())
    {
        pidMap.erase(key);
    }
}
std::string WatchDog::generateKey(const NodeInfo &nodeInfo)
{
    return nodeInfo.pkg + "-" + nodeInfo.type + "-" + nodeInfo.name;
}
void WatchDog::startWatchWork(int millSec)
{
    while (true)
    {
        watchNode();
        std::this_thread::sleep_for(std::chrono::microseconds(millSec));
    }
}
void WatchDog::watchNode(const NodeInfo &nodeInfo, pid_t pid)
{
    int status;
    pid_t result = waitpid(pid, &status, WNOHANG);
    if (0 == result)
    {
        // 节点正常运行，不可收回
        return;
    }
    // 节点不正常，可收回
    removePid(nodeInfo); // pid不再需要watch
    if (result != pid)
    {
        ROS_ERROR("waitpid fail, other pid: %ld", result); // 错误不用管
        return;
    }
    // 回收了指定pid的子进程
    std::string key = generateKey(nodeInfo);
    if (WIFEXITED(status)) // 子进程正常退出
    {
        ROS_INFO("node: %s, file: %s, state: %d, exit normally", key.c_str(), nodeInfo.execPath.c_str(), WEXITSTATUS(status));
    }
    else if (WIFSIGNALED(status)) // 进程被信号终止
    {
        ROS_ERROR("node: %s, file: %s, signal: %d", key.c_str(), nodeInfo.execPath.c_str(), WTERMSIG(status));
        recreateNode(nodeInfo); // 重新创建节点
    }
    else
    {
        // 其他状态
        ROS_WARN("process other state, %d", status);
    }
}
void WatchDog::watchNode()
{
    std::unordered_map<std::string, pid_t> pid_map = pidMap;
    auto iter = pid_map.begin();
    for (; iter != pid_map.end(); ++iter)
    {
        pid_t pid = iter->second;
        if (nodeInfoMap.find(iter->first) == nodeInfoMap.end())
        {
            ROS_ERROR("cannot find node info, pid: %ld, key: %s", pid, iter->first.c_str());
            continue;
        }
        NodeInfo nodeInfo = nodeInfoMap.at(iter->first);
        watchNode(nodeInfo, pid);
    }
}
